#include <MapManager.h>

#include <ros/package.h>
#include <tf/tf.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <MapIO.h>

#include <fstream>
#include <algorithm>
#include <numeric>
#include <sys/stat.h>

 namespace kybot_map {
MapManager::MapManager()
{

}

MapManager::~MapManager()
{
}

void MapManager::initialize()
{
    ros::NodeHandle privateNh("~");
    //发布话题
    locatingMapPublisher_ = privateNh.advertise<nav_msgs::OccupancyGrid>("locating_map", 1, true);
    planningMapPuber_ = privateNh.advertise<nav_msgs::OccupancyGrid>("/planning_map", 1, true);
    initialPosePuber_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, true);
    relocationflagPuber_ =  nh_.advertise<std_msgs::Bool>("/relocationflag", 1, true);
    laser_puber_ = privateNh.advertise<sensor_msgs::PointCloud2>("/map_manager_node/scan", 1, true);

    baseInfoSub_ = nh_.subscribe<kybot_msg::BaseInfo>("/base_info", 1, boost::bind(&MapManager::BaseInfoCallback, this, _1));
    //发布服务
    activeMapSrv_ = privateNh.advertiseService("active_map_srv", &MapManager::activeMapSrvCallback, this);
    getActivedMapNameSrv_ = privateNh.advertiseService("get_actived_map_name_srv", &MapManager::getActivedMapNameSrvCallback, this);
    setCurrentPoseSrv_ = privateNh.advertiseService("set_current_pose_srv", &MapManager::setCurrentPoseSrvCallback, this);
    getChargeStationPoseSrv_ = privateNh.advertiseService("get_chargestationpose_srv", &MapManager::getChargeStationPoseSrvCallback, this);
    setChargeStationPoseSrv_ = privateNh.advertiseService("set_chargestationpose_srv", &MapManager::setChargeStationPoseSrvCallback, this);
    node_info_srv_ = privateNh.advertiseService("node_info", &MapManager::NodeInfoSrvCallback, this);
    startLocalizationSrv_ = privateNh.advertiseService("start_localization", &MapManager::StartLocalization, this);
    get_location_statusSrv = privateNh.advertiseService("get_location_status", &MapManager::GetLocationStatus, this);
    get_current_mode_Srv = privateNh.advertiseService("get_current_mode", &MapManager::GetCurrentMode, this);
    Switch_Map_Srv = privateNh.advertiseService("Switch_Map", &MapManager::SwitchMapCallback, this);
    Search_Map_Srv = privateNh.advertiseService("Search_Map", &MapManager::SearchMapCallback, this);

    
    bootposefilename_ = ros::package::getPath("kybot_accurate_positioning")+"/data/Boot.json";

    isCharging = false;
    chargeButton = false;
    std::string map_manager_type;
    privateNh.param("MapManagerType", map_manager_type, std::string("location_software"));
    map_operation_ = kybot_map::MapOperation::CreateMapOperation(map_manager_type);
    map_info_ = kybot_map::MapInfo::CreateMapInfo(map_manager_type);
    privateNh.param("usecheckchargepose", usecheckchargepose_, false);
    privateNh.param("mappath", mappath_, std::string("/home/kybot/kybot_ws/PCD"));
    ROS_INFO("Map subsytem initialize complete");
    BootandlocationThread_ = boost::make_shared<boost::thread>(boost::bind(&MapManager::Boot_and_location, this));

    // 初始化 glog
    std::string username = std::getenv("USER");                              // 获取当前用户名
    std::string logdir = "/home/" + username + "/logdir/kybot_map/";  // 日志目录路径
    InitGlog(logdir, "kybot_map");
    
}

void MapManager::run()
{
    ros::spin();
}

// glog配置
void MapManager::InitGlog(const std::string& log_dir, const std::string& log_name) {
    google::InitGoogleLogging(log_name.c_str());
    google::InstallFailureSignalHandler();
    // 设置 glog 参数
    FLAGS_logbufsecs = 0;                    // 日志实时输出
    FLAGS_max_log_size = 10;                 // 最大日志文件大小 10M
    FLAGS_logtostderr = false;               // 日志消息不输出到标准错误
    FLAGS_stop_logging_if_full_disk = true;  // 磁盘满时停止日志记录
    FLAGS_colorlogtostderr = true;           // 设置记录到标准输出的颜色消息
    FLAGS_alsologtostderr = true;            // 日志消息同时输出到标准错误

    // 检查日志目录是否存在，如果不存在则创建
    if (access(log_dir.c_str(), 0) == -1) {
        std::string command = "mkdir -p " + log_dir;
        int result = system(command.c_str());
        if (result != 0) {
        LOG(ERROR) << "Failed to create log directory: " << log_dir;
        }
    }

    // 设置日志文件输出路径
    google::SetLogDestination(google::INFO, (log_dir + log_name).c_str());
    LOG(INFO) << "Glog ON";
}

bool MapManager::activeMapSrvCallback(kybot_msg::ActiveMapSrv::Request &req, kybot_msg::ActiveMapSrv::Response &res)
{
    const bool CALL_SUCCESS = true;
    res.failReason = "";

    MapIO::MapDataPtr mapPtr = MapIO::loadMap(req.name);
    if (mapPtr == NULL)
    {
        res.failReason = "noTargetMap";
        LOG(INFO)<<"Active map:%s fail, no target map"<<req.name.data();
    }
    else
    {
        publishNavigationMap(mapPtr);
        publishInitialPose(mapPtr->initialPose);
        LOG(INFO)<<"Active map:%s success"<<req.name.data();
    }

    return CALL_SUCCESS;
}

bool MapManager::getActivedMapNameSrvCallback(kybot_msg::GetActivedMapNameSrv::Request &req, kybot_msg::GetActivedMapNameSrv::Response &res)
{
    //res.name = activedMapName_;
    return true;
}

bool MapManager::setCurrentPoseSrvCallback(kybot_msg::SetCurrentPoseSrv::Request &req, kybot_msg::SetCurrentPoseSrv::Response &res)
{
    std_msgs::Bool msg;
    msg.data =  map_operation_->RelocatorPos(req.pose);
    relocationflagPuber_.publish(msg);
    res.failReason = map_operation_->GetLastFailReason();
    return true;
}

bool MapManager::getChargeStationPoseSrvCallback(kybot_msg::GetChargeStationPoseSrv::Request &req, kybot_msg::GetChargeStationPoseSrv::Response &res)
{
    const bool CALL_SUCCESS = true;
    // res.success = true;

    // MapMetaDataPtr pMapMetaData = MapIO::loadMapMetaData(activedMapName_);

    // if (pMapMetaData)
    // {
    //     res.pose = pMapMetaData->chargeStationPose;
    //     // res.pose.x = res.pose.x - 1.5 * cos(res.pose.theta);
    //     // res.pose.y = res.pose.y - 1.5 * sin(res.pose.theta);
    //     res.success = true;
    // }
    // else
    // {
    //     ROS_ERROR("can not load map meta data");
    //     res.success = false;
    // }

    return CALL_SUCCESS;
}

bool MapManager::setChargeStationPoseSrvCallback(kybot_msg::SetChargeStationPoseSrv::Request &req, kybot_msg::SetChargeStationPoseSrv::Response &res)
{
    const bool CALL_SUCCESS = true;
    // res.success = true;

    // MapMetaDataPtr pMapMetaData = MapIO::loadMapMetaData(activedMapName_);

    // if (pMapMetaData)
    // {
    //     pMapMetaData->chargeStationPose = req.pose;

    //     MapIO::saveMapMetaData(activedMapName_, pMapMetaData);
    // }
    // else
    // {
    //     res.success = false;
    // }

    return CALL_SUCCESS;
}

void MapManager::publishNavigationMap(MapIO::MapDataPtr mapDataPtr)
{
    nav_msgs::OccupancyGrid locatingMap, planningMap;
    locatingMap = mapDataPtr->map;
    planningMap.header = mapDataPtr->map.header;
    planningMap.info = mapDataPtr->map.info;

    //产生路径规划地图
    int width = mapDataPtr->map.info.width, height = mapDataPtr->map.info.height;
    cv::Mat image(height, width, CV_8UC1);
    ////获得基本地图，格式为cv::Mat
    for (int i = 0; i < height; i++)
        for (int j = 0; j < width; j++)
        {
            if (mapDataPtr->map.data[i * width + j] == 0)
            {
                image.at<uchar>(i, j) = 254; //opencv以图片左上角为原点,因此这里opencv中的图片其实是颠倒的
            }
            else if (mapDataPtr->map.data[i * width + j] == 100)
            {
                image.at<uchar>(i, j) = 0;
            }
            else
            {
                image.at<uchar>(i, j) = 205;
            }
        }
    ////地图虚拟墙绘制
    std::for_each(mapDataPtr->virtualWalls.begin(), mapDataPtr->virtualWalls.end(), [&image, &mapDataPtr, this](geometry_msgs::Polygon polygon) {
        if (polygon.points.size() > 1)
        {
            std::accumulate(++polygon.points.begin(), polygon.points.end(), *polygon.points.begin(), [&image, &mapDataPtr, this](geometry_msgs::Point32 &p1, geometry_msgs::Point32 &p2) {
                if (this->isWorldPointInMap(p1, mapDataPtr->map.info) && this->isWorldPointInMap(p2, mapDataPtr->map.info))
                {
                    cv::line(image, this->worldPointToCVPoint(p1, mapDataPtr->map.info), this->worldPointToCVPoint(p2, mapDataPtr->map.info), cv::Scalar(0), 2);
                }
                return p2;
            });
        }
    });
    ////将cv::Mat转为occupancyGrid
    planningMap.data.resize(height * width);
    for (int i = 0; i < height; i++)
        for (int j = 0; j < width; j++)
        {
            double occ = (255 - image.at<uchar>(i, j)) / 255.0; //opencv以图片左上角为原点,而occupangrid以图片左下角为原点.但由于在前面坐标系变换的时候没有进行颠倒,这里拷贝再进行一次颠倒,导致最后图片方向正常
            if (occ > MapIO::OCCUPIED_THRESH_)
            {
                planningMap.data[i * width + j] = 100;
            }
            else if (occ < MapIO::FREE_THRESH_)
            {
                planningMap.data[i * width + j] = 0;
            }
            else
            {
                planningMap.data[i * width + j] = -1;
            }
        }

    locatingMapPublisher_.publish(locatingMap);
    planningMapPuber_.publish(planningMap);
}

/*检查充电状态*/
void MapManager::BaseInfoCallback(const kybot_msg::BaseInfo::ConstPtr &msg)
{
    isCharging = msg->batteryCharging;
}
void MapManager::publishInitialPose(geometry_msgs::Pose2D pose)
{
    geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceStamped;
    poseWithCovarianceStamped.header.frame_id = "map";
    poseWithCovarianceStamped.header.stamp = ros::Time::now();
    poseWithCovarianceStamped.pose.pose.position.x = pose.x;
    poseWithCovarianceStamped.pose.pose.position.y = pose.y;
    poseWithCovarianceStamped.pose.pose.position.z = 0.0;
    poseWithCovarianceStamped.pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose.theta);
    poseWithCovarianceStamped.pose.covariance[0] = 0.01;  //x-x
    poseWithCovarianceStamped.pose.covariance[7] = 0.01;  //y-y
    poseWithCovarianceStamped.pose.covariance[35] = 0.01; //theta-theta
    initialPosePuber_.publish(poseWithCovarianceStamped);
}

    bool MapManager::StartLocalization(kybot_msg::StartLocalization::Request & req, 
                                        kybot_msg::StartLocalization::Response & res)
    {
        if (req.cmd == "start")
            map_operation_->StartLocalization(std::string());
        else
            map_operation_->StopLocalization(std::string());

        res.failReason = map_operation_->GetLastFailReason();
        return true;
    }

    bool MapManager::GetLocationStatus(kybot_msg::GetLocationStatusSrv::Request & req, 
                                        kybot_msg::GetLocationStatusSrv::Response & res)
    {
        res.x = GetPose().x;
        res.y = GetPose().y;
        res.yaw = GetPose().theta;
        res.status = GetLocalizationStatus();
        //res.failReason = map_opetarion_->GetLastFailReason();
        return true;
    }
 
    bool MapManager::GetCurrentMode(kybot_msg::GetCurrentModeSrv::Request & req, 
                                    kybot_msg::GetCurrentModeSrv::Response & res)
    {
        res.mode = GetMode();
        return true;
    }
    bool MapManager::SwitchMapCallback(kybot_msg::SwitchMapSrv::Request & req, 
        kybot_msg::SwitchMapSrv::Response & res)
    {
        std::string map_name = req.name;
        ROS_INFO("map_name:%s",map_name.c_str());
        if(map_operation_->SwitchMap(map_name))
        {
            ROS_INFO("SwitchMap success");
            res.result = "success";
            return true;
        }
        else
        {
            ROS_INFO("SwitchMap failed");
            res.result = "failed";
            return false;
        }


    }
    bool MapManager::SearchMapCallback(kybot_msg::SearchMapSrv::Request & req, 
        kybot_msg::SearchMapSrv::Response & res)
    {
        std::vector<std::string> mapNameList_;
        if(map_operation_->SearchMap(mapNameList_,mappath_))
        {
            res.mapnames = mapNameList_;
            ROS_INFO("mapNameList_.size:%d",mapNameList_.size());
            ROS_INFO("SwitchMap success");
            return true;
        }
        else
        {
            res.mapnames = mapNameList_;
            ROS_INFO("SwitchMap success");
            return true;
        }

    }
    bool MapManager::checkChargepose()
    {
        if(isCharging && !chargeButton)
        {
           chargeButton = true;
           std::ifstream ifile(bootposefilename_);
           geometry_msgs::Pose2D pose;
        //    JsonParser object;
           using json = nlohmann::json;
           json object;
           ifile >> object;
           ifile.close();
            pose.x = object["charge_pose"]["x"] ;
            pose.y = object["charge_pose"]["y"] ;
            pose.theta = object["charge_pose"]["theta"] ;
            if(fabs(pose.x  -  GetPose().x)>0.1 || fabs(pose.y  - GetPose().y)>0.1 >0.1 || fabs(pose.theta  - GetPose().theta) > 0.17) //0.17 - 10°
            {
                map_operation_->RelocatorPos(pose);
                sleep(1);
            }
        }
        if(!isCharging)
        {
            chargeButton = false;
        }
        return true;
    }
    bool MapManager::Shutdownsaveposeinfile()
    {
        struct stat buf;
        // JsonParser object;
        using json = nlohmann::json;
        json object;
        std::ifstream ifile(bootposefilename_);
        stat(bootposefilename_.c_str(), &buf);
        int filesize = buf.st_size;
        if(!ifile.good())
        {
            std::ofstream newfile(bootposefilename_);
            if(newfile.good())
            {
                object["bootbefore_pose"]["x"] = map_info_->GetPose().x;
                object["bootbefore_pose"]["y"] = map_info_->GetPose().y;
                object["bootbefore_pose"]["theta"]=map_info_->GetPose().theta;
                newfile << object.dump(2);
                newfile.close();
                return true;       
            }
            else
            {
                LOG(INFO)<<"creat file error";
                return false;
            }            
        }
        else
        {
            if(filesize > 0)
            {
                ifile >> object;
                ifile.close();
                object["bootbefore_pose"]["x"] = map_info_->GetPose().x;
                object["bootbefore_pose"]["y"] = map_info_->GetPose().y;
                object["bootbefore_pose"]["theta"]=map_info_->GetPose().theta;
                //ROS_INFO("map_info_->GetPose().x:%f",map_info_->GetPose().x);
                std::ofstream ofile(bootposefilename_);
                ofile << object.dump(2);
                ofile.close();
                return true;
            }
            else
            {
                object["bootbefore_pose"]["x"] = map_info_->GetPose().x;
                object["bootbefore_pose"]["y"] =  map_info_->GetPose().y;
                object["bootbefore_pose"]["theta"]= map_info_->GetPose().theta;
                //ROS_INFO("map_info_->GetPose().x:%f",map_info_->GetPose().x);
                std::ofstream ofile(bootposefilename_);
                ofile << object.dump(2);
                ofile.close();
                return true;
            }
              
        }
    }
    bool MapManager::Bootinitpose()
    {
        struct stat buf;
        geometry_msgs::Pose2D pose;
        // JsonParser object;
        using json = nlohmann::json;
        json object;
        std::ifstream ifile(bootposefilename_);
        stat(bootposefilename_.c_str(), &buf);
        int filesize = buf.st_size;
        if(!ifile.good())
        {
            return false;                           
        }
        else
        {
            if(filesize > 0)
            {
            ifile >> object;
            ifile.close();
            pose.x = object["bootbefore_pose"]["x"] ;
            pose.y = object["bootbefore_pose"]["y"] ;
            pose.theta = object["bootbefore_pose"]["theta"] ;
            map_operation_->RelocatorPos(pose);
            LOG(INFO)<<"init pose";
            return true;
            }
            else
            {
                LOG(INFO)<<"error no pose data";
                return false;
            }
        }

    }
    void MapManager::Boot_and_location()
    {
        try
        {
            if(Bootinitpose())
            {
                LOG(INFO)<<"initpose success";
            }
            else
            {
                if(Shutdownsaveposeinfile())
                {
                    LOG(INFO)<<"init boot pose data files";
                }
            }
            sleep(1);
            ros::Rate loop_rate(1);
            while (ros::ok())
            {
                if(usecheckchargepose_)
                checkChargepose();
                if(Shutdownsaveposeinfile())
                {

                }
                else
                    LOG(INFO)<<"save pose error";
                loop_rate.sleep();
            }
        }
        catch (boost::thread_interrupted &)
        {
            LOG(INFO)<<"Boot_and_location thread was interrupted";
            return;
        }
    }

     bool MapManager::NodeInfoSrvCallback(kybot_msg::NodeInfoSrv::Request &req, 
							kybot_msg::NodeInfoSrv::Response &res)
    {
        //res.compilation_time = COMPILATION_TIME;
        return true;
    }

 }

